#include "HeliCtrlPanel.h"
#include "hal.h"
#include "protocol_parser.h"


BEGIN_EVENT_TABLE(HeliCtrlPanel, MainPanel)
	EVT_JOYSTICK_EVENTS(HeliCtrlPanel::OnJoystickEvent)
	EVT_TIMER(TELEMETRY_TIMER_ID, HeliCtrlPanel::OnTelemetryTimer)
	EVT_TIMER(TEST_TIMER_ID, HeliCtrlPanel::OnTestTimer)
END_EVENT_TABLE()

HeliCtrlPanel::HeliCtrlPanel( wxWindow* parent )
	: MainPanel( parent ), m_TelemetryTimer(this,TELEMETRY_TIMER_ID), m_TestTimer(this,TEST_TIMER_ID)
{
	m_Joystick = NULL;
	m_CurrentJoy = -1;
	FillJoys();	
	InitTelemetry();
	com_port->SetSelection( 0 );
	for(int i=1;i<10;i++)
	{
		com_port->Append(wxString::Format(wxT("COM%i"),i));
	}

	m_Config = new wxFileConfig(_T("HeliCtrl"));
	LoadConfig();

	Connect_ButtonClick(* new wxCommandEvent());	
}

HeliCtrlPanel::~HeliCtrlPanel()
{
	SaveConfig();
	delete m_Config;
	FreeJoy();
}


void HeliCtrlPanel::FillJoys()
{	
	int count = wxJoystick().GetNumberJoysticks();
	m_CurrentJoy = -1;

	m_JoyList->Clear();

	for (int i = 0; i < count; i++) 
	{
		wxJoystick joystick(i);
		m_JoyList->AppendString(joystick.GetProductName());
	}

	if(count > 0)
	{
		m_JoyList->SetSelection(0);
		RunJoy(0);
	}
	else
	{
		FreeJoy ();
	}
}

void HeliCtrlPanel::RunJoy (int n)
{
	if(m_CurrentJoy == n)//don`t change identical joy
		return;

	FreeJoy ();

	m_Joystick = new wxJoystick(n);
	m_Joystick->SetCapture(this, 30);

	m_CurrentJoy = n;

	m_JoyStatus->SetColour(awxLED_GREEN);
	m_JoyStatus->SetState(awxLED_ON);

}

void HeliCtrlPanel::FreeJoy ()
{
	if(m_Joystick != NULL)
	{
		m_JoyStatus->SetColour(awxLED_RED);
		m_JoyStatus->SetState(awxLED_ON);

		//m_Joystick->ReleaseCapture();
		delete m_Joystick;

		m_Joystick = NULL;
	}
}

void HeliCtrlPanel::InitTelemetry()
{
	m_TelemEng1 = m_TelemetryChart->AppendNewCurve();
	m_TelemetryChart->SetCurvePen(m_TelemEng1,*wxRED_PEN);

	m_TelemEng2 = m_TelemetryChart->AppendNewCurve();
	m_TelemetryChart->SetCurvePen(m_TelemEng2,wxPen(wxColor(_("MEDIUM BLUE"))));


	m_TelemetryChart->SetXLabel(_("Time [s]"));
	m_TelemetryChart->SetYLabel(_("Power 1 & 2 [%]"));
	m_TelemetryChart->ShowUnits();	
	m_TelemetryChart->SetUnits(1, 1);
	m_TelemetryChart->SetChartXYUnits(100,350);
	m_TelemetryChart->SetOffset(0,-20);
	m_TelemetryChart->UpdateChart();


	m_Telem1Eng1 = m_TelemetryChart1->AppendNewCurve();
	m_TelemetryChart1->SetCurvePen(m_Telem1Eng1,*wxRED_PEN);
	
	m_TelemetryChart1->SetXLabel(_("Time [s]"));
	m_TelemetryChart1->SetYLabel(_("Bat.power[V]"));
	m_TelemetryChart1->ShowUnits();	
	m_TelemetryChart1->SetUnits(1, 1);
	m_TelemetryChart1->SetChartXYUnits(300,10);
	m_TelemetryChart1->SetOffset(0,0);
	m_TelemetryChart1->UpdateChart();	


	m_Telem2Curv1 = m_TelemetryChart2->AppendNewCurve(2000);
	m_TelemetryChart2->SetCurvePen(m_Telem2Curv1,*wxRED_PEN);

	m_Telem2Curv2 = m_TelemetryChart2->AppendNewCurve(2000);
	m_TelemetryChart2->SetCurvePen(m_Telem2Curv2,wxPen(wxColor(_("MEDIUM BLUE"))));
	
	m_TelemetryChart2->SetXLabel(_("Time [s]"));
	m_TelemetryChart2->SetYLabel(_("Integr."));
	m_TelemetryChart2->ShowUnits();	
	m_TelemetryChart2->SetUnits(1, 1);
	m_TelemetryChart2->SetChartXYUnits(300,125);
	m_TelemetryChart2->SetOffset(0,-50);
	m_TelemetryChart2->UpdateChart();	
}

void HeliCtrlPanel::LoadConfig(void)
{
	long comIndex = 1;
	m_Config->Read(_T("ComIndex"),&comIndex);
	com_port->SetSelection (comIndex);
	 
	long yaw = 8;
	m_Config->Read(_T("EnginesPrescaler"),&yaw);
	yaw_correction->SetValue((int)yaw);

	long vlimit = 700;
	m_Config->Read(_T("VoltageLimit"),&vlimit);
	voltage_limit->SetValue((int)vlimit);
	vll->SetLabel(wxString::Format(_T("Limit: %4.2f [V]"),(float)vlimit/100));

	long beeptype = 1;
	m_Config->Read(_T("BeepType"),&beeptype);
	beep_type->SetValue((int)beeptype);


	long integroffx = 50;
	m_Config->Read(_T("IntegrOffsetX"),&integroffx);
	integr_offsets_x->SetValue(integroffx);

	long integroffy = 50;
	m_Config->Read(_T("IntegrOffsetY"),&integroffy);
	integr_offsets_y->SetValue(integroffy);

}

void HeliCtrlPanel::SaveConfig(void)
{
	m_Config->Write(_T("ComIndex"),com_port->GetSelection());
	m_Config->Write(_T("EnginesPrescaler"),yaw_correction->GetValue());
	m_Config->Write(_T("VoltageLimit"),voltage_limit->GetValue());
	m_Config->Write(_T("BeepType"),beep_type->GetValue());
	m_Config->Write(_T("IntegrOffsetX"),integr_offsets_x->GetValue());
	m_Config->Write(_T("IntegrOffsetY"),integr_offsets_y->GetValue());
}


void HeliCtrlPanel::OnOK( wxCommandEvent& event )
{

}

void HeliCtrlPanel::Connect_ButtonClick( wxCommandEvent& event )
{ 
	if(m_ConnectB->GetLabel() == _T("Connect"))
	{
	telemetry_snap_shot->ClearGrid();
	if(telemetry_snap_shot->GetRows() > 0)
		telemetry_snap_shot->DeleteRows(0,100);
	HalInit(com_port->GetStringSelection().wchar_str());
	m_ConnectB->SetLabel(_T("Disconnect"));
	m_TelemetryTimer.Start(300);
	//m_TestTimer.Start(1);
	m_ConnStatus->SetColour(awxLED_GREEN);
	m_ConnStatus->SetState(awxLED_ON);
	}
	else
	{
	HalDeInit();
	m_ConnectB->SetLabel(_T("Connect"));
	m_TelemetryTimer.Stop();
	m_ConnStatus->SetColour(awxLED_RED);
	m_ConnStatus->SetState(awxLED_ON);
	}
}


void HeliCtrlPanel::Test_ButtonClick( wxCommandEvent& event )
{

HalTest();
}

void HeliCtrlPanel::OnBeepBtn( wxCommandEvent& event )
{	
	if(beep_btn->GetValue())
		StartBeep((unsigned char)beep_type->GetValue());
	else
		StopBeep();
}

void HeliCtrlPanel::OnVoltageLimit(wxScrollEvent &event)
{
	double voltage;
	voltage = (double) event.GetPosition() / 100;

	wxString msg = wxString::Format(_T("Limit: %4.2f [V]"),voltage);

	vll->SetLabel(msg);

	SetLowVoltageLimit(voltage);
}

void HeliCtrlPanel::OnIntegrOffsets(wxScrollEvent &event)
{	
	SetIntergatorZerosOffsets((unsigned short)integr_offsets_x->GetValue(),
		(unsigned short)integr_offsets_y->GetValue());	
}

void HeliCtrlPanel::OnIntegrReset( wxCommandEvent& event )
{
	ResetIntegrators();
}


void HeliCtrlPanel::OnJoysReload( wxCommandEvent& event )
{
	FillJoys();
}

void HeliCtrlPanel::OnJoyChange( wxCommandEvent& event )
{
	if(m_JoyList->GetCurrentSelection() >= 0)
		RunJoy(m_JoyList->GetCurrentSelection());	
}

void HeliCtrlPanel::OnJoystickButtonEvent(int Button, bool Down)
{
	if(Button==1) EnablePowerForEngines(Down);
	if((Button==2) && Down) 
	{
		double ThrustOnTailEngine_YawControl, ThrustOnMainEngine_AltitudeControl;
		GetHelicopterThrust(&ThrustOnTailEngine_YawControl, &ThrustOnMainEngine_AltitudeControl);
		/* create telemetry snapshot */
		telemetry_snap_shot->AppendRows();
		telemetry_snap_shot->SetCellValue(telemetry_snap_shot->GetRows()-1,0,wxString::Format(wxT("%f"),ThrustOnTailEngine_YawControl));
		telemetry_snap_shot->SetCellValue(telemetry_snap_shot->GetRows()-1,1,wxString::Format(wxT("%f"),ThrustOnMainEngine_AltitudeControl));
	}
	if((Button==3) && Down)
	{
		beep_btn->SetValue(true);
		OnBeepBtn(*new wxCommandEvent());
	}
	if((Button==3) && !Down)
	{
		beep_btn->SetValue(false);
		OnBeepBtn(*new wxCommandEvent());
	}
}


void HeliCtrlPanel::OnJoystickEvent(wxJoystickEvent& event)
{
	//wxLogDebug(_("Status: [%d,%d]"),event.GetPosition().x,event.GetPosition().y);	
	if(event.IsButton())
	{
		/* 4  */ if(event.GetButtonChange()&0x8) OnJoystickButtonEvent(4,event.GetButtonState()&0x8?1:0);
		/* 2  */ if(event.GetButtonChange()&0x2) OnJoystickButtonEvent(2,event.GetButtonState()&0x2?1:0);
		/* 3  */ if(event.GetButtonChange()&0x4) OnJoystickButtonEvent(3,event.GetButtonState()&0x4?1:0);
		/*shot*/ if(event.GetButtonChange()&0x1) OnJoystickButtonEvent(1,event.GetButtonState()&0x1?1:0);
	}

	if(event.IsZMove())
	{
		//wxLogDebug(_("ZMove: [%d]"),event.GetZPosition());
	}

	if(event.IsMove())
	{	//update ctrl
		m_JoyDraw->SetJoyPosition(event.GetPosition());	


		//update machine
		double yawCor = (double)yaw_correction->GetValue();
		wxVertex2D scaled;
		wxPoint diff;
		diff = m_PrevJoyPos - event.GetPosition();

		if( abs(diff.x) < 10 && abs(diff.y) < 10)
			return;

		m_PrevJoyPos = event.GetPosition();

		scaled=PrescaleToPercentx10(event.GetPosition());
		double pes = scaled.y/320;
		
		yawCor = yawCor - pes;

		double v1,v2;
		v1 = (1000-scaled.x)/(yawCor);		
		v2 = scaled.y/10;

		if(v1 > 100)
			v1 = 100;
		if(v1 < 50)
			v1 = 50;
		
		SetHelicopterThrust(v1,v2);
		wxLogDebug(_("Status: [%u,%d]"),(unsigned short)event.GetPosition().x,event.GetPosition().y);
		//wxLogDebug(_("Pos: [%f,%f]"),v1,v2);
	}
}

wxVertex2D HeliCtrlPanel::PrescaleToPercentx10(wxPoint &point)
{
		wxVertex2D scaled;

		unsigned short x,y;
		x = (unsigned short)point.x;
		y = (unsigned short)point.y;

		scaled.x = (int) (((float)x/0xffff) *1000); 
		scaled.y = (int) (((float)y/0xffff) *1000); 

		return scaled;
}

void HeliCtrlPanel::OnTelemetryTimer(wxTimerEvent& event)
{
	double ThrustOnTailEngine_YawControl, ThrustOnMainEngine_AltitudeControl,Battery_Voltage;
	static double tick;
	unsigned long Integr_Meas_Ch1,Integr_Meas_Ch2;

	tick+=0.1f;

	GetHelicopterThrust(&ThrustOnTailEngine_YawControl, &ThrustOnMainEngine_AltitudeControl);
	
	m_TelemetryChart->AddendCurveData(m_TelemEng1,wxVertex2D(tick,ThrustOnTailEngine_YawControl));
	m_TelemetryChart->AddendCurveData(m_TelemEng2,wxVertex2D(tick,ThrustOnMainEngine_AltitudeControl));
	m_TelemetryChart->ScrollChart(m_TelemEng1);

	
	Battery_Voltage = GetHelicopterAnalogMeasure(BATTERY_VOLTAGE);
	m_TelemetryChart1->AddendCurveData(m_Telem1Eng1,wxVertex2D(tick,Battery_Voltage));
	m_TelemetryChart1->ScrollChart(m_Telem2Curv1);

	battery_voltage->SetLabel(wxString::Format(wxT("Battery: %2.2f [V]"),GetHelicopterAnalogMeasure(BATTERY_VOLTAGE)));
	acl1->SetLabel(wxString::Format(wxT("accelX: %f"),GetHelicopterAnalogMeasure(ACCELX)));
	acl2->SetLabel(wxString::Format(wxT("accelY: %f"),GetHelicopterAnalogMeasure(ACCELY)));

	Integr_Meas_Ch1 = GetHelicopterIntegratedMeasures(0);
	Integr_Meas_Ch2 = GetHelicopterIntegratedMeasures(1);
	m_TelemetryChart2->AddendCurveData(m_Telem2Curv1,wxVertex2D(tick,(double)Integr_Meas_Ch1/0x4fffffL));
	m_TelemetryChart2->AddendCurveData(m_Telem2Curv2,wxVertex2D(tick,(double)Integr_Meas_Ch2/0x4fffffL));
	//m_TelemetryChart2->ScrollCurveEnd_X(m_Telem2Curv1);	
	//m_TelemetryChart2->UpdateChart(1000);	
	m_TelemetryChart2->ScrollChart(m_Telem2Curv1);
	
}

void HeliCtrlPanel::OnTestTimer(wxTimerEvent& event)
{


}
